package es.uji.viselab.experiment;

import es.uji.viselab.LogLevel;
import es.uji.viselab.LogRecorder;
import es.uji.viselab.benchmark.database.DBConnection;
import es.uji.viselab.image.process.CameraParameters;
import es.uji.viselab.image.process.ImageProcessing;
import es.uji.viselab.image.process.cameras.VirtualCameraParameter;
import es.uji.viselab.math.Pose;
import es.uji.viselab.math.Vector2;
import es.uji.viselab.math.Vector3;
import es.uji.viselab.network.DataPackage;
import es.uji.viselab.robot.Robot;
import es.uji.viselab.robot.RobotException;

public class Experiment1 extends Experiment {

	int iterations;
	double[] velocities;
	protected Vector2[] targetCoordinateList;
	protected Vector2[] imageCoordinateList;

	public Experiment1() {
		this.camParams = new VirtualCameraParameter();
	}

	@Override
	public Vector3[] getModel3d() {
		Vector3 p1 = new Vector3(1.05 + 0.1, 0.05 + 0.1, 0);
		Vector3 p2 = new Vector3(1.05 + 0.1, -0.05 + 0.1, 0);
		Vector3 p3 = new Vector3(0.95 + 0.1, -0.05 + 0.1, 0);
		Vector3 p4 = new Vector3(0.95 + 0.1, 0.05 + 0.1, 0);
		Vector3[] points = { p2, p3, p4, p1 };
		// Vector3[] points = { p1};
		return points;
	}

	@Override
	public Vector2[] getTarged2d() {
		Vector2 p1 = new Vector2(200, 200);
		Vector2 p2 = new Vector2(200, -200);
		Vector2 p3 = new Vector2(-200, -200);
		Vector2 p4 = new Vector2(-200, 200);
		Vector2[] points = { p1, p2, p3, p4 };
		// Vector2[] points = { p1};

		return points;
	}

	@Override
	public void cameraFeaturePointsChanged(ImageProcessing i) {
		targetCoordinateList = i.getTargetCoordinateList();
		imageCoordinateList = i.getImageCoordinateList();
		velocities = visualServoing.compute(i);
	}

	double getError() {
		double sumError = 0;
		int numPoints = targetCoordinateList.length;
		for (int i = 0; i < numPoints; i++) {
			sumError = sumError
					+ Math.sqrt(Math.pow(targetCoordinateList[i].x - imageCoordinateList[i].x, 2)
							+ Math.pow(targetCoordinateList[i].y - imageCoordinateList[i].y, 2));
		}
		sumError = sumError / numPoints;
		return sumError;
	}

	@Override
	public void Start() {
		iterations = 1;
		try {
			robot.connect();
		} catch (RobotException e1) {
			// TODO Auto-generated catch block
			e1.printStackTrace();
		}
		DBConnection db = benchmark.getDatabase();
		experimentId = db.setNewExperiment();
		LogRecorder.log(LogLevel.INFO, "New experiment with id " + experimentId + " started ");

		// TODO control also the error, not only the iterations
		while (iterations < 20) {
			try {

				Pose pose = robot.getPose();
				DataPackage dn = new DataPackage();
				dn.setType("XYZ_END_EFFECTOR");
				dn.setSequence(iterations);
				dn.addValue(pose.getX());
				dn.addValue(pose.getY());
				dn.addValue(pose.getZ());

				db.insert(dn);

				dn = new DataPackage();
				dn.setType("ERROR");
				dn.setSequence(iterations);
				dn.addValue(iterations);
				double error = getError();
				dn.addValue(error);

				db.insert(dn);

				robot.move(velocities);
			} catch (RobotException e) {
				e.printStackTrace();
			}
			iterations++;
		}
	}

	@Override
	public void newConnectionStablished() {
		// TODO Auto-generated method stub

	}

	@Override
	public void newDataArrived(DataPackage dn) {
		// TODO Auto-generated method stub

	}



}
